/**
 * The Woosta Table Football Engine
 *
 * License... etc.
 **
 * Physics Engine
 *
 * The physics engine operates in several stages:
 * (1) User input, force & velocity updates
 * 	This takes place outside the physics engine. The
 *	game logic processes input from the user and updates
 *	object's forces and velocity with respect to that.
 * (2) Rigid body dynamics
 *	Objects which are interacting with other objects must
 *	have internal forces applied at their 'connection points',
 *	based upon the forces and velocities above.
 * (3) Initial collision detection
 *	Based upon the initial forces and velocity given above,
 * 	the collision detection task begins and builds a list
 *	of collisions in order of time.
 * (4) Collision processing
 *	Collisions are processed in order of time, by moving the
 *	colliding objects along to the point of collision (hence
 *	shifting the time frame) and creating a temporary connection
 *	between the two objects. A collision event is fired, so additional
 *	game logic (1) may be invoked. Finally, the affected systems
 *	and objects must be updated again (2), and collision detection
 *	re-run (3) from the new perspective and time frame.
 * (5) Final update
 *	After we can be sure there will be no more collisions in
 *	this frame, we simply move objects to their positions at
 *	the end of the frame. Physics processing is complete.
 **
 * Author: Sean Micklethwaite
 **/

#include "physics.h"
#ifdef __PHYSICS_INTERNAL__

using namespace WtfEngine;

std::set<PhysicalSystem*> PhysicalSystem::gSystems;

#define DELTA_THRESHOLD		0.1

/**** PhysicalModel ****/

PhysicalModel::~PhysicalModel(){
	if(mzLocalVertex){
		delete [] mzLocalVertex;
	};
};

/**
 * Creates a texture object to hold the vertex data,
 * initialises physical properties.
 **/
void PhysicalModel::Create(int numVertices, tScalar density)
{
	tScalar mass = 0;

	//this->FBOTexture::Create(1, numVertices);
	//this->Write((void*)mzLocalVertex);
	mNumVertices = numVertices;

	
	//
	// Calculate local inverse inertia tensor based upon
	// given vertex data
	//

	int		i;
	tScalar	pointMass;

	mLocalInverseInertiaTensor = Orientation3D(0);
	mResistancePoints[0] = tVector();
	mResistancePoints[1] = tVector();
	
	for(i = 0; i < numVertices; i++){
		pointMass = mzLocalVertex[i].length() * density;
		mass += pointMass;

		mLocalInverseInertiaTensor[0][0] += pointMass *
			(SQR(mzLocalVertex[i][1]) + SQR(mzLocalVertex[i][2]));
		mLocalInverseInertiaTensor[1][1] += pointMass *
			(SQR(mzLocalVertex[i][0]) + SQR(mzLocalVertex[i][2]));
		mLocalInverseInertiaTensor[2][2] += pointMass *
			(SQR(mzLocalVertex[i][0]) + SQR(mzLocalVertex[i][1]));

		mLocalInverseInertiaTensor[0][1] += pointMass *
			(mzLocalVertex[i][0] * mzLocalVertex[i][1]);
		mLocalInverseInertiaTensor[0][2] += pointMass *
			(mzLocalVertex[i][0] * mzLocalVertex[i][2]);
		mLocalInverseInertiaTensor[1][2] += pointMass *
			(mzLocalVertex[i][1] * mzLocalVertex[i][2]);
		
		
		//
		// Calculate highest and lowest points in each dimension
		//
		
		if(mResistancePoints[0][0] < mzLocalVertex[i][0])
			mResistancePoints[0][0] = mzLocalVertex[i][0];
		else if(mResistancePoints[1][0] > mzLocalVertex[i][0])
			mResistancePoints[1][0] = mzLocalVertex[i][0];
		
		if(mResistancePoints[0][2] < mzLocalVertex[i][2])
			mResistancePoints[0][2] = mzLocalVertex[i][2];
		else if(mResistancePoints[1][2] > mzLocalVertex[i][2])
			mResistancePoints[1][2] = mzLocalVertex[i][2];
		
		if(mResistancePoints[0][1] < mzLocalVertex[i][1])
			mResistancePoints[0][1] = mzLocalVertex[i][1];
		else if(mResistancePoints[1][1] > mzLocalVertex[i][1])
			mResistancePoints[1][1] = mzLocalVertex[i][1];
	};

	mLocalInverseInertiaTensor[1][0] = mLocalInverseInertiaTensor[0][1];
	mLocalInverseInertiaTensor[2][0] = mLocalInverseInertiaTensor[0][2];
	mLocalInverseInertiaTensor[2][1] = mLocalInverseInertiaTensor[1][2];
	mLocalInverseInertiaTensor = mLocalInverseInertiaTensor.inverse();

	mResistancePoints[0] = mResistancePoints[0] * 0.5;
	mResistancePoints[1] = mResistancePoints[1] * 0.5;
	
	mMassQuot = 1 / (mass);
};


void PhysicalModel::UpdateModel(const Orientation3D &ori,
	const tVector& vel, const Orientation3D &angVel, tScalar dt, tVector *zvVertex, tVector *zvNextVertex) const
{
	int i;
	for(i = 0; i < mNumVertices; i++){
		zvVertex[i] = mzLocalVertex[i]; // TODO: no need for this - always specified as object local
		zvNextVertex[i] = ((ori.identity() + (angVel * dt)).orthonormalize() * mzLocalVertex[i]) + (ori.transpose() * vel * dt);
	};
};


/// pushes the worldview matrix and performs the render
void Renderable::Render() { 
	tScalar			zRotation[16];

	// TODO: Use a 4x4 matrix for current orientation and use a transpose OpenGL function
	// to perform the multiplication.
	zRotation[0] = currentOrientation[0][0];
	zRotation[1] = currentOrientation[1][0];
	zRotation[2] = currentOrientation[2][0];
	zRotation[3] = 0;
	zRotation[4] = currentOrientation[0][1];
	zRotation[5] = currentOrientation[1][1];
	zRotation[6] = currentOrientation[2][1];
	zRotation[7] = 0;
	zRotation[8] = currentOrientation[0][2];
	zRotation[9] = currentOrientation[1][2];
	zRotation[10] = currentOrientation[2][2];
	zRotation[11] =	0;
	zRotation[12] =	currentPosition[0];
	zRotation[13] = currentPosition[1];
	zRotation[14] = currentPosition[2];
	zRotation[15] = 1;

	glPushMatrix();
	glMultMatrixd(zRotation);
	
	this->data->render();

	glPopMatrix();
};

Static::Static(const WtfGraphics::GraphicsData::tRef& d)
	: Renderable3D(d) {
	PhysicsEngine::GetInstance().PushStatic(this);
};

void Static::Destroy(){
	PhysicsEngine::GetInstance().RemoveStatic(this);
	this->Renderable3D::Destroy();
};

void PhysicsEngine::Run()
{
	tActiveList::iterator								iter;
	tScalar	dt = Kernel::GetInstance().GetGameState()->GetDelta();
	
	//
	// Update velocities & positions (for active objects)
	//

	for(iter = mActiveList.begin();
		iter != mActiveList.end();
		++iter)
	{
		(*iter)->UpdateForces();
		(*iter)->UpdatePhysical(dt, true);
	};
};


Physical::Physical(const WtfGraphics::GraphicsData::tRef d,
			const PhysicalModel::tRef m, bool bParticle)
	: Renderable3D(d), isActive(false), mModel(m), mLocalTime(0),
	mAngularVelocity(0), isParticle(bParticle), mNumInteractions(0)
{
	this->UpdatePhysical(0, true);
	PhysicsEngine::GetInstance().PushPhysical(this);
	m->AllocateVertexArrays(mzVertex, mzNextVertex);
};

void Physical::Destroy() {
	PhysicsEngine::GetInstance().RemovePhysical(this);
	this->Renderable3D::Destroy();
};

void Physical::UpdateForces() {
	std::set<Force::tRef>::iterator	f;

	this->currentForce = tVector();
	this->mTorque = tVector();

	for(f = this->attachedForces.begin();
		f != this->attachedForces.end();
		++f)
	{
		this->applyForce((**f), (*f)->relativePosition);
	};

	if(this->currentForce.isZero() && this->mTorque.isZero()
		&& this->isActive
		&& this->currentVelocity.isZero())
		PhysicsEngine::GetInstance().SetPassive(this);
	else if(!this->isActive
		&& !(this->currentForce.isZero() && this->mTorque.isZero()))
		PhysicsEngine::GetInstance().SetActive(this);
};

void Physical::IntegrateForces(tScalar dt)
{

};


void Physical::UpdatePhysical(tScalar dtTot, bool endFrame)
{
	tScalar dt;
	int nSubSteps, step;
	
	// Trim down time delta if we have already moved
	dtTot -= mLocalTime;

	if(endFrame){
		// End of frame - reset counters
		mLocalTime = 0;
		mNumInteractions = 0;
	}else
		mLocalTime += dtTot;
	
	//
	// If the time delta is too large, break the integration into smaller steps.
	//
	
	nSubSteps = 1;
	dt = dtTot / nSubSteps;
	for(step = 0;
		step < nSubSteps; step++)
	{
		// Calculate air resistance
		/*tVector pointVel;
		for(int i = 0; i < 2; i++){
			pointVel = ((mAngularVelocity.identity() + mAngularVelocity) * mModel->getResistancePoints()[i]) + currentVelocity - mModel->getResistancePoints()[i];
			pointVel = -pointVel * dt;
			
			applyImpulse(tVector(0, pointVel[1], pointVel[2]) * mModel->getResistancePoints()[i][0],
						tVector(mModel->getResistancePoints()[i][0], 0, 0));
			applyImpulse(tVector(pointVel[0], 0, pointVel[2]) * mModel->getResistancePoints()[i][1],
						tVector(0, mModel->getResistancePoints()[i][1], 0));
			applyImpulse(tVector(pointVel[0], pointVel[1], 0) * mModel->getResistancePoints()[i][2],
						tVector(0, 0, mModel->getResistancePoints()[i][2]));
		};*/
		
		// Update position
		this->currentPosition += (this->currentVelocity * dt);
	
		// Update orientation
		currentOrientation += (mAngularVelocity * dt);
		currentOrientation = currentOrientation.orthonormalize();
		
		// Integrate to get updated linear velocity
		this->currentVelocity += (this->currentForce * getMassQuot() * dt);
		this->mLastVelocity = this->currentVelocity;
	
		// Update angular momentum
		mAngularMomentum += (mTorque * dt);
	
		// Calculate angular velocity
		mInverseInertiaTensor = currentOrientation * mModel->getLocalInverseInertiaTensor()
				* currentOrientation.transpose();
		mAngularVelocity = currentOrientation
				.getDerivative(mInverseInertiaTensor * mAngularMomentum);
	};
};

/**
 * Applies a world-local force to the object
 **/
void Physical::applyForce(tVector force, tVector pos){
	currentForce += force;
	mTorque += pos.cross(currentOrientation * force);
	
	if(currentForce.isZero() && mTorque.isZero() && currentVelocity.isZero() && mAngularMomentum.isZero()) {
		if(isActive) {
			PhysicsEngine::GetInstance().SetPassive(this);
			isActive = false;
		};
	} else {
		if(!isActive) {
			PhysicsEngine::GetInstance().SetActive(this);
			isActive = true;
		};
	};
};

/**
 * Applies an OBJECT-local impulse to the object
 **/
void Physical::applyImpulse(tVector i, tVector p)
{
	currentVelocity += (currentOrientation * i * getMassQuot());
	mAngularMomentum += p.cross(i);
	
	if(currentForce.isZero() && mTorque.isZero() && currentVelocity.isZero() && mAngularMomentum.isZero()) {
		if(isActive) {
			PhysicsEngine::GetInstance().SetPassive(this);
			isActive = false;
		};
	} else {
		if(!isActive) {
			PhysicsEngine::GetInstance().SetActive(this);
			isActive = true;
		};
	};
};

void Physical::setVelocity(const tVector& v) {
	currentVelocity = v;

	if(currentForce.isZero() && mTorque.isZero() && currentVelocity.isZero() && mAngularMomentum.isZero()) {
		if(isActive) {
			PhysicsEngine::GetInstance().SetPassive(this);
			isActive = false;
		};
	} else {
		if(!isActive) {
			PhysicsEngine::GetInstance().SetActive(this);
			isActive = true;
		};
	};
};
void Physical::setAngularMomentum(const tVector& mom) {
	mAngularMomentum = mom;

	if(currentForce.isZero() && mTorque.isZero() && currentVelocity.isZero() && mAngularMomentum.isZero()) {
		if(isActive) {
			PhysicsEngine::GetInstance().SetPassive(this);
			isActive = false;
		};
	} else {
		if(!isActive) {
			PhysicsEngine::GetInstance().SetActive(this);
			isActive = true;
		};
	};
};

void Physical::setActive(){
	if(!isActive)
		PhysicsEngine::GetInstance().SetActive(this);
};

#endif
